
#pragma once

#include "SLAM/common.hpp"
#include <memory>
#include <string>
#include <vector>
#include <deque>
#include <limits>
#include <algorithm>
#include <chrono>
#include <cmath>
#include <numeric>


#include <tf2_ros/create_timer_ros.h>  //V1.1
#include <rmw/qos_profiles.h>  //v1.1
#include <rclcpp/rclcpp.hpp>
#include <sensor_msgs/msg/point_cloud2.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <cartographer_ros_msgs/msg/landmark_list.hpp>

#include <Eigen/Core>

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/message_filter.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <message_filters/subscriber.h>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>

#include <ceres/ceres.h>

#include <pcl_conversions/pcl_conversions.h>
#include <pcl/filters/passthrough.h>
#include <pcl/segmentation/extract_clusters.h>
#include <pcl/search/kdtree.h>

#include <tf2/utils.h>
#include <tf2/LinearMath/Quaternion.h>
// 匈牙利算法头
#include "HungarianAlgorithm.hpp"
>>>>>>> origin/reflectorDetector
#include "params_detector.hpp"
namespace reflector_slam_system {

class ReflectorDetector : public rclcpp::Node {
public:
  using PointT = pcl::PointXYZI;
  using CloudT = pcl::PointCloud<PointT>;

  explicit ReflectorDetector(const rclcpp::NodeOptions& opts = rclcpp::NodeOptions{} , paramsDetector params = paramsDetector{})
  : rclcpp::Node("reflector_detector_node", opts)
  {
    params_ = params;
  };

/*
  // ===== TF & IO =====
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

  //不进行坐标系转换，不进行时间对齐
  rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr sub_scan_;

  std::shared_ptr<message_filters::Subscriber<sensor_msgs::msg::PointCloud2, rclcpp::Node>> sub_cloud_;


  std::shared_ptr<tf2_ros::MessageFilter<sensor_msgs::msg::PointCloud2>> tf_filter_;

  rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr sub_pose_;
  rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr pub_markers_;
  rclcpp::Publisher<cartographer_ros_msgs::msg::LandmarkList>::SharedPtr pub_landmarks_;

  // ===== Callbacks =====
  void onCloud(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg);
  void cbPose(const geometry_msgs::msg::PoseStamped::SharedPtr msg);
*/
  // ===== Pipeline =====
  void passThroughZ(CloudT::Ptr cloud);
  void coarseFilter(const CloudT::Ptr& in, CloudT::Ptr out, std::vector<size_t>& kept_idx);
  void fineFilterInBins(const CloudT::Ptr& in, const std::vector<size_t>& kept_idx, std::vector<int>& mask_keep);

  bool fitCircleCeres(const CloudT::Ptr& cloud, const std::vector<int>& idx,
                      Eigen::Vector2d& center, double& radius, double& avg_I) const;

  void clusterAndFit(const CloudT::Ptr& in, const std::vector<int>& mask_keep,
                     std::vector<Eigen::Vector2d>& centers,
                     std::vector<double>& radii,
                     std::vector<double>& strengths);

  void associateAndUpdate(const std::vector<Eigen::Vector2d>& centers,
                          const std::vector<double>& radii,
                          const std::vector<double>& strengths);

  void removeOldLandmarks(const rclcpp::Time& current_time);

  // 地标发布（在 map 下）
  void publishLandmarksInMap(const geometry_msgs::msg::TransformStamped tf,
                            const std::vector<Eigen::Vector2d>& centers);


  // 点坐标转换
  bool transformPointToMap(double x_laser, double y_laser,
                          const geometry_msgs::msg::TransformStamped tf,
                          double& x_map, double& y_map);

  // 统计分桶（返回桶数、同时输出 rmin/rmax/mu/sd）
  int calculateMuAndSd(const CloudT::Ptr& in,
                       std::vector<double>& mu, std::vector<double>& sd,
                       double& rmin, double& rmax);

  // 打印累计到目前为止的“全局最小/最大距离及其强度”
  void printGlobalIntensityDistanceStats(const sensor_msgs::msg::PointCloud2::ConstSharedPtr& msg) ;

private:
  paramsDetector params_;
  
};

} // namespace reflector_slam_system

